#include "posture.h"
#include "drive/MPU6050/mpu6050.h"
#include <stdio.h>
#include <string.h>
#include "utils/Quaternion.h"
#include "tts.h"
#include "./drive/sys_timer.h"
float posture_init[3];
Quaternion post_init;
static uint8_t status_ = POSTRUE_ERR;
Quaternion PostureGetQ() {
    float q[4] = {0};
    uint32_t temp_time = CurMillis() + 1000;
    while (mpu_get_quaternion(q) != 0) {
      if (temp_time < CurMillis()) {
        break;
      }
    }
    return QuaterCreat(q[0], q[1], q[2], q[3]);
}

void PostureInit() {
    if (MPU_Init()) {
        status_ = POSTRUE_ERR;
        TTS(TTS_MPU_ERR);
        return ;
    }
    status_ = POSTRUE_OK;
    PostureReset();
}

void PostureReset() {
  post_init = PostureGetQ();
}

float PostureCurYaw() {
    float var[3];
    Quaternion cur = PostureGetQ();
    Quaternion diff = QuaterDiff(cur, post_init);
    QuaterToEuler(diff, var);
    return var[YAW];
}

uint8_t PostureStatus() {
  return status_;
}



